#include "bilateral_normal_filter.h"

bilateral_normal_filter::bilateral_normal_filter()
{
    this->polygon = new MPolygon;
    IteNormalUpdate = 0;
    IteVertexUpdate = 0;
    Variance_of_Centroids = 0.0;
    Variance_of_Normals = 0.0;
}

bilateral_normal_filter::bilateral_normal_filter(const MPolygon * poly)
{
    this->polygon = new MPolygon;
//    this->polygon = &(*poly);
    this->polygon->points.clear();
    this->polygon->normals.clear();

    int md = poly->PointNo();
    for(int i = 0; i < poly->PointNo(); i++){
        this->polygon->points.push_back(poly->points[i]);

        Point2d p1(poly->points[i].x,
                   poly->points[i].y);
        Point2d p2(poly->points[(i+1)%md].x,
                   poly->points[(i+1)%md].y);

        Vector2d tt(p1, p2);
        double len = tt.length();
        if(len<1e-10)
            return;

        tt.rotate90();
        this->polygon->normals.push_back(tt);
    }

    IteNormalUpdate = 0;
    IteVertexUpdate = 0;
    Variance_of_Centroids = 0.0;
    Variance_of_Normals = 0.0;
}

void bilateral_normal_filter::VertexUpdate(int iter)
{
    int iteration = 0;

    if(polygon->PointNo() == 0)
        return;
    int md = polygon->PointNo();
    if(polygon->normals.size() != polygon->PointNo()){
        for(int i = 0; i < polygon->PointNo(); i++){

            Point2d p1(polygon->points[i].x,
                       polygon->points[i].y);
            Point2d p2(polygon->points[(i+1)%md].x,
                       polygon->points[(i+1)%md].y);

            Vector2d tt(p1, p2);
            double len = tt.length();
            if(len<1e-10)
                return;

            tt /= len;
            tt.rotate90();
            this->polygon->normals.push_back(tt);
        }
    }


    // start
    while(iteration < iter){
        for(int i = 0; i < polygon->PointNo(); i++)
        {

            Point2d p0(polygon->points[(i-1+md)%md].x/2.0,
                       polygon->points[(i-1+md)%md].y/2.0);
            Point2d p1(polygon->points[i].x/2.0,
                       polygon->points[i].y/2.0);
            Point2d p2(polygon->points[(i+1)%md].x/2.0,
                       polygon->points[(i+1)%md].y/2.0);


            Vector2d vec0(p1, p0);
            Vector2d vec1(p1, p2);
            double inner1 = vec1.inner_product(polygon->normals[i]);
            double inner0 = vec0.inner_product(polygon->normals[(i-1+md)%md]);

            polygon->points[i].x += 1.0/2.0*(polygon->normals[i].x*(inner1));
            polygon->points[i].x += 1.0/2.0*(polygon->normals[(i-1+md)%md].x*(inner0));

            polygon->points[i].y += 1.0/2.0*(polygon->normals[i].y*(inner1));
            polygon->points[i].y += 1.0/2.0*(polygon->normals[(i-1+md)%md].y*(inner0));
        }
    }

}

void bilateral_normal_filter::NormalUpdate(double varv, double varn, int iter)
{
    int iteration = 0;

    if(polygon->PointNo() == 0)
        return;

    int md = polygon->PointNo();
    std::vector<std::vector<double> > nbc(md, std::vector<double>(2, 0.0));
    std::vector<std::vector<double> > nbn(md, std::vector<double>(2, 0.0));


    if(polygon->normals.size() != polygon->PointNo()){
        for(int i = 0; i < polygon->PointNo(); i++){

            Point2d p1(polygon->points[i].x,
                       polygon->points[i].y);
            Point2d p2(polygon->points[(i+1)%md].x,
                       polygon->points[(i+1)%md].y);

            Vector2d tt(p1, p2);
            double len = tt.length();
            if(len<1e-10)
                return;

            tt /= len;
            tt.rotate90();
            this->polygon->normals.push_back(tt);
        }
    }


    for(int i = 0; i < polygon->PointNo(); i++){

        Point2d p1((polygon->points[(i+1)%md].x + polygon->points[(i+2)%md].x) / 2.0,
                   (polygon->points[(i+1)%md].y + polygon->points[(i+2)%md].y) / 2.0);
        Point2d p((polygon->points[(i)%md].x + polygon->points[(i+1)%md].x) / 2.0,
                   (polygon->points[(i)%md].y + polygon->points[(i+1)%md].y) / 2.0);
        Point2d p0((polygon->points[(i)%md].x + polygon->points[(i-1+md)%md].x) / 2.0,
                   (polygon->points[(i)%md].y + polygon->points[(i-1+md)%md].y) / 2.0);

        Vector2d t1(p, p0);
        Vector2d t2(p, p1);

        double a1 = t1.length();
        double a2 = t2.length();
        nbc[i][0] = exp(-(a1*a1 / 2.0 / varv/varv));
        nbc[i][1] = exp(-(a2*a2 / 2.0 / varv/varv));


    }

    std::vector<double> wt(polygon->PointNo(), 0.0);
    // start
    while(iteration < iter){
        for(int i = 0; i < polygon->PointNo(); i++){
            Vector2d n1 = polygon->normals[(i)%md] - polygon->normals[(i+1)%md];
            Vector2d n0 = polygon->normals[(i)%md] - polygon->normals[(i-1+md)%md];

            Point2d p1(polygon->points[(i-1)%md].x,
                       polygon->points[(i-1)%md].y);
            Point2d p2(polygon->points[(i)%md].x ,
                       polygon->points[(i)%md].y);
            Point2d p3(polygon->points[(i+1)%md].x,
                       polygon->points[(i+1)%md].y);
            Point2d p4(polygon->points[(i+2)%md].x,
                       polygon->points[(i+2)%md].y);


            double a1 = n0.length();
            double a2 = n1.length();

            Vector2d t1(p1,p2);
            Vector2d t2(p2,p3);
            Vector2d t3(p3,p4);

            nbn[i][0] = exp(-(a1*a1 / 2.0 / varn/varn));
            nbn[i][1] = exp(-(a2*a2 / 2.0 / varn/varn));

            wt[i] = t1.length()*nbc[i][0] * nbn[i][0] +
                    t2.length() +
                    t3.length()*nbc[i][1] * nbn[i][1];
        }

        for(int i = 0; i < polygon->PointNo(); i++)
        {

            Vector2d vec0 = polygon->normals[(i-1+md)%md];
            Vector2d vec1 = polygon->normals[(i)%md];
            Vector2d vec2 = polygon->normals[(i+1)%md];
            double w0 = nbc[i][0] * nbn[i][0];
            double w2 = nbc[i][1] * nbn[i][1];

            double l0 = polygon->Segment((i-1+md)%md).length();
            double l1 = polygon->Segment((i-0+md)%md).length();
            double l2 = polygon->Segment((i+1+md)%md).length();

            polygon->normals[i]= wt[i] * (vec0*w0*l0 + vec1*l1 + vec2*w2*l2);

        }

    }
}

